#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
import time

class XWheelRobot:
    #速度
    _speed = 60
  
    #标记运行状态
    isRunning = False

    #ID列表
    _l1Id = 25
    _l3Id = 21   
    _r1Id = 22
    _r3Id = 20   

    #初始化机器人：使用 BwRobotLib 对象
    def __init__(self,mclib):
        self.lib = mclib

    #绑定ID与模型 L1 L3 ; R1 R3
    def bindIds(self,ids):
        self._l1Id = ids[0]
        self._l3Id = ids[1]

        self._r1Id = ids[2]
        self._r3Id = ids[3]

        for id in ids:
            self.lib.SetMotorMode(id,1)

    def goForward(self):
        print("向前走")
        self.isRunning = True
        self.lib.SetMotorTargetSpeed(self._r3Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._l3Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._r1Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._l1Id, self._speed)
        pass

    def goBack(self):
        print("向后走")
        self.isRunning = True
        self.lib.SetMotorTargetSpeed(self._r3Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._l3Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._r1Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._l1Id, -self._speed)
        pass

    def turnLeft(self):
        print("左转")
        self.isRunning = True
        self.lib.SetMotorTargetSpeed(self._r3Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._l3Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._r1Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._l1Id, -self._speed)
        pass

    def turnRight(self):
        print("右转")
        self.isRunning = True
        self.lib.SetMotorTargetSpeed(self._r3Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._l3Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._r1Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._l1Id, self._speed)
        pass


    def runLeft(self):
        print("向左走")
        self.isRunning = True
        self.lib.SetMotorTargetSpeed(self._r3Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._l3Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._r1Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._l1Id, -self._speed)
        pass


    def runRight(self):
        print("向右走")
        self.isRunning = True
        self.lib.SetMotorTargetSpeed(self._r3Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._l3Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._r1Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._l1Id, self._speed)
        pass


    
    def runRightUp(self):
        print("向右上走")
        self.isRunning = True
        self.lib.SetMotorTargetSpeed(self._r3Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._l3Id, 0)
        self.lib.SetMotorTargetSpeed(self._r1Id, 0)
        self.lib.SetMotorTargetSpeed(self._l1Id,  self._speed)
        pass

    def runRightDown(self):
        print("向右下走")
        self.isRunning = True
        self.lib.SetMotorTargetSpeed(self._r3Id,0)
        self.lib.SetMotorTargetSpeed(self._l3Id, -self._speed)
        self.lib.SetMotorTargetSpeed(self._r1Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._l1Id, 0)
        pass


    def runLeftUp(self):
        print("向左上走")
        self.isRunning = True
        self.lib.SetMotorTargetSpeed(self._r3Id, 0)
        self.lib.SetMotorTargetSpeed(self._l3Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._r1Id,-self._speed)
        self.lib.SetMotorTargetSpeed(self._l1Id, 0)
        pass

    def runLeftDown(self):
        print("向左下走")
        self.isRunning = True
        self.lib.SetMotorTargetSpeed(self._r3Id, self._speed)
        self.lib.SetMotorTargetSpeed(self._l3Id,0)
        self.lib.SetMotorTargetSpeed(self._r1Id,0)
        self.lib.SetMotorTargetSpeed(self._l1Id, -self._speed)
        pass

    def stop(self):
        print("停止")
        self.isRunning = False
        self.lib.SetMotorTargetSpeed(self._r3Id,0)
        self.lib.SetMotorTargetSpeed(self._l3Id,0)
        self.lib.SetMotorTargetSpeed(self._r1Id,0)
        self.lib.SetMotorTargetSpeed(self._l1Id,0)
        pass

  